#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <gmm_voxel_map/gmm_voxel_map.h>


int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "gmm_test");
    ros::NodeHandle nh("~");
	int component_num=7;

	gvm::GMM other_gmm;

	std::vector<Eigen::Vector3d> means;
    std::vector<Eigen::Matrix3d> var;

    std::vector<double> pi_vector;
	double pi[7]={	0.2683578133583069, 0.20160213112831116, 0.1535380482673645, 0.14953270554542542, 0.12283044308423996, 0.07476635277271271, 0.02937249094247818};


	for(int i=0;i<7;i++)
	{
		pi_vector.push_back(pi[i]);
	}

	Eigen::Matrix<double,7,3> means_matrix;
	means_matrix<<
	-17.724811553955078, -1.5246411561965942, -0.05285476893186569,

		-15.566707611083984, 1.838722825050354, -0.03404288738965988, 

		-17.46442985534668, 3.2286128997802734, -0.09600413590669632, 

		-16.870952606201172, 1.8428869247436523, -0.014999997802078724, 
		
		-16.445512771606445, 4.849052906036377, 0.010465843603014946, 
		
		-17.739999771118164, -0.14000000059604645, -0.014999998733401299, 
		
		-17.576364517211914, -2.6354544162750244, 0.073636360466480;

	for(int i=0;i<7;i++)
	{
		means.push_back(means_matrix.row(i));
	}

	Eigen::Matrix<double,21,3> var_matrix;
	var_matrix<<
	0.029470764100551605, -0.0029874288011342287, 0.001102910260669887,
	 -0.0029874288011342287, 0.030129477381706238, 0.0005846566637046635, 
	 0.001102910260669887, 0.0005846566637046635, 0.3534667193889618, 
	 
	 
	 0.022288013249635696, -0.0008952286443673074, -0.0034673206973820925, 
	 -0.0008952286443673074, 0.02278572879731655, 0.0012904105242341757, 
	 -0.0034673206973820925, 0.0012904105242341757, 0.35964640974998474, 
	 
	 0.015853192657232285, 0.00027986912755295634, 0.008181028068065643, 
	 0.00027986912755295634, 0.016242703422904015, 0.0015450619393959641, 
	 0.008181028068065643, 0.0015450619393959641, 0.3297860026359558, 
	 
	 
	 0.012543822638690472, 0.0024514507967978716, 0.00013393549306783825, 
	 0.0024514507967978716, 0.018841560930013657, 0.0012388419127091765, 
	 0.00013393549306783825, 0.0012388419127091765, 0.37062498927116394, 
	 
	 
	 0.021528074517846107, -0.012088778428733349, 0.0072430274449288845, 
	 -0.012088778428733349, 0.02252565510571003, -0.0009066181373782456, 
	 0.0072430274449288845, -0.0009066181373782456, 0.270974725484848, 
	 
	 
	 0.007500018924474716, -3.478740160699525e-17, -1.279358480814834e-17, 
	 -3.483233074914668e-17, 0.007499999832361937, -2.7253743335966468e-17, 
	 -1.2390881853094249e-17, -2.6516487182165305e-17, 0.37062498927116394, 
	 
	 0.009814085438847542, -0.007562010549008846, -0.0015495920088142157, 
	 -0.007562010549008846, 0.017479337751865387, 0.0019834693521261215, 
	 -0.0015495920088142157, 0.0019834693521261215, 0.14549587666988373;

	for(int i=0;i<7;i++)
	{
		var.push_back(var_matrix.block(i*3,0,3,3));
	}
	
	other_gmm.init(pi_vector,means,var,means.size(),Eigen::Vector3d::Zero());
	int samplenum=100;
	std::vector<Eigen::Vector3d>	samplePoints=other_gmm.GMMRandomSamplePoints(samplenum,1.6);
	ROS_INFO("samplePoints.SIZE():%ld",samplePoints.size());

	for(int i=0;i<samplePoints.size();i++)
	{
		ROS_INFO("SAMPLE POINT: %f %f %f",samplePoints[i](0),samplePoints[i](1),samplePoints[i](2));
	}


}
